#include <ros/ros.h>
#include <geometry_msgs/TwistStamped.h>

int main(int argc, char** argv)
{
  ros::init(argc, argv, "servo_control_demo");
  ros::NodeHandle nh;

  // Publisher to control the robot end-effector velocity
  ros::Publisher twist_pub = nh.advertise<geometry_msgs::TwistStamped>("servo_server/delta_twist_cmds", 10);

  // Set publish rate
  ros::Rate loop_rate(50);  // 50 Hz

  while (ros::ok())
  {
    // Create a Twist message
    geometry_msgs::TwistStamped cmd;

    // Set frame_id to match your robot's frame
    cmd.header.frame_id = "base_link";

    // Set linear velocities (m/s)
    cmd.twist.linear.x = 0.1;  // Move forward
    cmd.twist.linear.y = 0.0;
    cmd.twist.linear.z = 0.0;

    // Set angular velocities (rad/s)
    cmd.twist.angular.x = 0.0;
    cmd.twist.angular.y = 0.0;
    cmd.twist.angular.z = 0.1;  // Rotate around z-axis

    // Publish the command
    twist_pub.publish(cmd);

    // Spin and sleep
    ros::spinOnce();
    loop_rate.sleep();
  }

  return 0;
}
